import rospy
from sensor_msgs.msg import NavSatFix
from math import sin,cos,sqrt,atan2
import scipy.io
PI = 3.1415

x_log = []
y_log = []
latitude_log = []
longitude_log = []

class coor:
  def __init__(self):
    self.latitude = 0.0
    self.longitude = 0.0
    self.x = 0.0
    self.y = 0.0
  
  def reset_me(self):
    self.latitude = 0.0
    self.longitude = 0.0
    self.x = 0.0
    self.y = 0.0

  def copy_to_me(self,var):
    self.latitude = var.latitude
    self.longitude = var.longitude
    self.x = var.x
    self.y = var.y

def calcDistance(p1,p2):

  R = 6371
  lat1 = p1.latitude*PI/180
  lat2 = p2.latitude*PI/180
  dLat = lat2 - lat1
  dLon = (p2.longitude - p1.longitude)*PI/180
  
  a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2)
  c = 2 * atan2(sqrt(a), sqrt(1-a))
  d = R*c
  
  return d*1000


def calcBearing(p1,p2):

  lat1 = p1.latitude*PI/180
  lat2 = p2.latitude*PI/180
  dLat = lat2 - lat1
  dLon = (p2.longitude - p1.longitude)*PI/180
  y = sin(dLon) * cos(lat2)
  x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon)
  brng = atan2(y, x)
  
  return brng

def save_matlab(data,name,dir=None):
        print "saving ", name
        str = name + ".mat"
        scipy.io.savemat(str, mdict={name: data},oned_as='row')

class rosClass:
  def __init__(self):
    rospy.init_node('egps')
    self._init_coor = coor()
    self._coor = coor()
    self._Ecoor = coor()
    self._ctr = 0
    self._first = True
    self._sub = rospy.Subscriber('/SENSORS/GPS',NavSatFix,self.gpsCallback)
    
  def gpsCallback(self,data):
    if self._first:
      self._first = False
      self._init_coor.latitude = data.latitude
      self._init_coor.longitude = data.longitude
      self._Ecoor.latitude = data.latitude
      self._Ecoor.longitude = data.longitude
    self._coor.latitude = data.latitude
    self._coor.longitude = data.longitude
    
    d = calcDistance(self._coor,self._init_coor)
    b = calcBearing(self._coor,self._init_coor)
    
    self._coor.x = d * cos(b)
    self._coor.y = d * sin(b)
    
    self.updateEcoor()

  def updateEcoor(self):
    a = self._Ecoor.x*self._ctr
    b = a + self._coor.x
    c = b/(self._ctr + 1)
    self._Ecoor.x = c
    
    self._Ecoor.y = (self._Ecoor.y*self._ctr + self._coor.y)/(self._ctr+1)
    self._Ecoor.latitude = (self._Ecoor.latitude*self._ctr + self._coor.latitude)/(self._ctr+1)
    self._Ecoor.longitude = (self._Ecoor.longitude*self._ctr + self._coor.longitude)/(self._ctr+1)
    self._ctr += 1
    #print "ctr#: %d|  x= %.2f y= %.2f | x= %.2f y= %.2f"%(self._ctr,self._Ecoor.x,self._Ecoor.y,self._coor.x,self._coor.y)
    
gps = rosClass()
T = 0.05
num = 600
try:
  while(True):
    rospy.sleep(T)
    print "ctr#: %d|  x= %.2f y= %.2f"%(gps._ctr,gps._Ecoor.x,gps._Ecoor.y)
    x_log.append(gps._coor.x)
    y_log.append(gps._coor.y)
    latitude_log.append(gps._coor.latitude)
    longitude_log.append(gps._coor.longitude)

    if gps._ctr == num:
      num += 100
      print "Resetting init coordinate"
      gps._init_coor.copy_to_me(gps._Ecoor)
      gps._Ecoor.reset_me()
      gps._ctr = 0
      
except:
  save_matlab(x_log,'x')
  save_matlab(y_log,'y')
  save_matlab(latitude_log,'lat')
  save_matlab(longitude_log,'long')
  
